#!/usr/bin/env python
# MPC Planner configuration

from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, bool_t
from local_planner_limits import add_generic_localplanner_params

gen = ParameterGenerator()

# This unusual line allows to reuse existing parameter definitions
# that concern all localplanners
add_generic_localplanner_params(gen)

gen.add("debug_info", bool_t, 0, "Debug information", False)
gen.add("delay_mode", bool_t, 0, "Delay mode", True)
gen.add("max_speed", double_t, 0, "Maximum speed [m/s]", 0.50, 0.01, 5.0)
gen.add("waypoints_dist", double_t, 0, "Waypoint distance [m]", -1, -1.0, 10.0)
gen.add("path_length", double_t, 0, "Path length [m]", 5.0, 0.0, 10.0)
gen.add("controller_freq", double_t, 0, "Controller Frequency", 10.0, 0.01, 100.0)

gen.add("mpc_steps", double_t, 0, "Steps", 20.0, 0.0, 100.0)
gen.add("mpc_ref_cte", double_t, 0, "Reference Cross Track Error", 0.0, 0.0, 10.0)
gen.add("mpc_ref_vel", double_t, 0, "Reference Velocity", 1.0, 0.0, 5.0)
gen.add("mpc_ref_etheta", double_t, 0, "Reference Error theta", 0.0, 0.01, 5.0)

gen.add("mpc_w_cte", double_t, 0, "Weight of Cross Track Error", 1000.0, 0.01, 5000.0)
gen.add("mpc_w_etheta", double_t, 0, "Weight of Error Theta", 1000.0, 0.01, 5000.0)
gen.add("mpc_w_vel", double_t, 0, "Weight of Velocity", 100.0, 0.01, 1000.0)
gen.add("mpc_w_angvel", double_t, 0, "Weight of Angular velocity", 100.0, 0.01, 1000.0)
gen.add("mpc_w_angvel_d", double_t, 0, "Weight of Angular velocity difference", 0.0, 0.01, 1000.0)
gen.add("mpc_w_accel", double_t, 0,"Weight of Acceleration", 50.0, 0.01, 1000.0)
gen.add("mpc_w_accel_d", double_t, 0,"Weight of Acceleration difference", 10.0, 0.01, 1000.0)

gen.add("mpc_max_angvel", double_t, 0, "Maximum Angular velocity", 1.0, 0.01, 5.0)
gen.add("mpc_max_throttle", double_t, 0, "Maximum Throttle", 1.0, 0.01, 5.0)
gen.add("mpc_bound_value", double_t, 0, "Bound value", 1000.0, 0.01, 1000.0)


exit(gen.generate("mpc_waypoint_tracking_controller", "mpc_waypoint_tracking_controller", "MPCWayPointTrackerROS"))